/**
 * @file /kobuki_driver/src/driver/packet_finder.cpp
 *
 * @brief Packet handling implementation.
 *
 * License: BSD
 *   https://raw.github.com/yujinrobot/kobuki_core/hydro-devel/kobuki_driver/LICENSE
 **/

/*****************************************************************************
** Includes
*****************************************************************************/

#include <sstream>
#include "packet_handler/imu_packet_finder.hpp"

/*****************************************************************************
** Namespaces
*****************************************************************************/

/*****************************************************************************
** Implementation
*****************************************************************************/

ImuPacketFinderBase::ImuPacketFinderBase() :
    state(waitingForStx), verbose(false)
{
}

/*****************************************************************************
** Public
*****************************************************************************/

void ImuPacketFinderBase::configure()
{
  size_stx = 2;
  //size_max = 5;
  STX.push_back(0x7A);
  STX.push_back(0x7B);

  clear();
}

void ImuPacketFinderBase::clear()
{
  state = waitingForStx;
  buffer.clear();
}

void ImuPacketFinderBase::enableVerbose()
{
  verbose = true;
}

bool ImuPacketFinderBase::checkSum()
{
  return true;
}

unsigned int ImuPacketFinderBase::numberOfDataToRead()
{
  unsigned int num(0);

  switch (state)
  {
    case waitingForPayloadSize:
      num = size_length_field;
      break;
    case waitingForStx:
      num = 1;
      break;
    case waitingForDataSize:
      num = 1;
      break;

    case clearBuffer:
    default:
      num = 1;
      break;
  }

  if (verbose)
  {
    printf("[state(%d):%02d]", state, num);
  }
  return num;
}

void ImuPacketFinderBase::getBuffer(BufferType & bufferRef)
{
  bufferRef = buffer;
}

/**
 * Checks for incoming packets.
 *
 * @param incoming
 * @param numberOfIncoming
 * @return bool : true if a valid incoming packet has been found.
 */
bool ImuPacketFinderBase::update(const unsigned char * incoming, unsigned int numberOfIncoming)
{
  if (!(numberOfIncoming > 0))
    return false;

  bool found_packet(false);

  if ( state == clearBuffer ) {
    buffer.clear();
    state = waitingForStx;
  }
  // Serial.print("state = ");
  // Serial.print(state);
  // Serial.print("\n");  
  switch (state)
  {
    case waitingForStx:
      if (WaitForStx(incoming[0]))
      {
        state = waitingForDataSize; // kobukibot
      }
      break;
    case waitingForDataSize:
      if (waitForDataSize(incoming[0])) 
      {
        state = waitingForPayloadSize;
      }
      break;
    case waitingForPayloadSize:
      if (waitForPayloadSize(incoming, numberOfIncoming, found_packet))
      {
        state = clearBuffer;
      }
      break;

    default:
      state = waitingForStx;
      break;
  }
  if ( found_packet ) {
    return checkSum();	//what happen if checksum is equal to false(== -1)?
  } else {
    return false;
  }
}
/*****************************************************************************
** Protected
*****************************************************************************/

bool ImuPacketFinderBase::WaitForStx(const unsigned char datum)
{
  bool found_stx(true);

  // add incoming datum
  buffer.push_back(datum);

  // Serial.print("buffer = ");
  // Serial.print(buffer.size());
  // Serial.print("\n");
  // Serial.print("STX.size() = ");
  // Serial.print(STX.size());
  // Serial.print("\n");
  // check whether we have STX
  for (unsigned int i = 0; i < buffer.size() && i < STX.size(); i++)
  {
    // Serial.print("WaitForStx = \n");
    // Serial.print(buffer[i]);
    // Serial.print("\n");
    if (buffer[i] != STX[i])
    {
      found_stx = false;
      // buffer.pop_front();
      buffer.pop_front();
      break;
    }
  }

  return (found_stx && buffer.size() == STX.size());
}

bool ImuPacketFinderBase::waitForDataSize(const unsigned char size)
{
  size_length_field = size;
  buffer.push_back(size);
  return true;
}

bool ImuPacketFinderBase::waitForPayloadSize(const unsigned char * incoming, unsigned int numberOfIncoming, bool & foundPacket)
{
  // push data
  unsigned char first_byte;
  for (unsigned int i = 0; i < numberOfIncoming; i++) {
    first_byte = incoming[i];
    buffer.push_back(incoming[i]);
  }

  // check when we need to wait for etx
  //(注:0x12 为报文字节数,除却 0x7A 与 0x7B 外整条报文共有 18 个字节,故为 0x12)
  if (buffer.size() < (size_stx + size_length_field))
  {
    return false;
  }
  else
  {
    foundPacket = true;
    return true;
  }
}
